/**
 ******************************************************************************
 * @file    mini_dog.c
 * @author  GEEKROS,  site:www.geekros.com
 ******************************************************************************
 */
 
 #include "mini_dog.h"
 
 Mini_Dog_Struct Mini_Dog_Data;

/******************************************************************
  * @ 函数名  ： Mini_Dog_Init
  * @ 功能说明： 机器狗初始化
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Init(void)
{

	// 设置舵机串口波特率
	Servo_Init(1000000);
	
	// 设置舵机类型
	Servo_Type(0, SERVO_SCS);
	
	// 设置电池低电压报警阀值
	Adc_Low_Voltage(11.1);
	
	// 从Flash中读取标定数据
	Mini_Dog_Get_Calibration_Data();
	
	// 初始化PID
	Mini_Dog_Data.Pitch_Pid.kp = Mini_Dog_Data.Roll_Pid.kp = 1.5;
	Mini_Dog_Data.Pitch_Pid.ki = Mini_Dog_Data.Roll_Pid.ki = 0.1;
	Mini_Dog_Data.Pitch_Pid.kd = Mini_Dog_Data.Roll_Pid.kd = 0.5;
	
	// 初始化目标姿态
	Mini_Dog_Data.Target.x = 0; // 重心
	Mini_Dog_Data.Target.z = 100; // 高度
	Mini_Dog_Data.Target.pitch = 0; // 俯仰角
	Mini_Dog_Data.Target.roll = 0; // 滚转角
	
	// 初始化当前姿态
	Mini_Dog_Data.Current.x = 0; // 重心
	Mini_Dog_Data.Current.z = 100; // 高度
	Mini_Dog_Data.Current.pitch = 0; // 俯仰角
	Mini_Dog_Data.Current.roll = 0; // 滚转角
	
	// 机械结构初始化
	Mini_Dog_Data.Body_Length = 140.f;
	Mini_Dog_Data.Body_Width = 126.f;
	Mini_Dog_Data.Foot_Width = 127.f;
	Mini_Dog_Data.Thigh_Length = 85.f;
	Mini_Dog_Data.Calf_Length = 78.f;
	
	// 其他配置初始化
	Mini_Dog_Data.Gait_Time = 0; // 步态时序
	Mini_Dog_Data.Gait_Cycle = 20; // 步态周期
	Mini_Dog_Data.Status = MINI_DOG_INIT_FALSE; // 初始化状态
	Mini_Dog_Data.Mode = MINI_DOG_REMOTE_CONTROL; // 控制模式
	Mini_Dog_Data.Motion_Mode = MINI_DOG_STOP; // 运动模式
	Mini_Dog_Data.Gait_Mode = MINI_DOG_TROT; // 步态模式
	
	// 机器人站立
	Mini_Dog_Data.Current.z = 35;
	Mini_Dog_Attitude_Interface("z", 35, 100, 500);
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Get_Calibration_Data
  * @ 功能说明： 从FLASH中获取零位标定数据
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Get_Calibration_Data(void)
{
	uint16_t Flash_Read_Data[8];
	Flash_Read(8, Flash_Read_Data);
	if(Flash_Read_Data[0] > 0){
		Mini_Dog_Data.Zero_Position[0] = Flash_Read_Data[0] - MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[1] = Flash_Read_Data[1] - MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[2] = Flash_Read_Data[2] + MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[3] = Flash_Read_Data[3] + MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[4] = Flash_Read_Data[4] + MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[5] = Flash_Read_Data[5] + MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[6] = Flash_Read_Data[6] - MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[7] = Flash_Read_Data[7] - MINI_DOG_OFFSET;
	}else{
		// 首次安装的机械结构零位数据
		Mini_Dog_Data.Zero_Position[0] = 511 - MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[1] = 511 - MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[2] = 511 + MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[3] = 511 + MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[4] = 511 + MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[5] = 511 + MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[6] = 511 - MINI_DOG_OFFSET;
		Mini_Dog_Data.Zero_Position[7] = 511 - MINI_DOG_OFFSET;
	}
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Get_Calibration_Data
  * @ 功能说明： 自稳PID计算
  * @ 参数    ： float mpu_val mpu 传感器
  * @ 参数    ： float target_val 目标值
  * @ 参数    ： Mini_Dog_Pid_Struct *pid PID
  * @ 返回值  ： NULL
  *****************************************************************/
float Mini_Dog_Stable_Calculation(float mpu_val, float target_val, Mini_Dog_Pid_Struct *pid)
{
	// 如果为自稳模式
	if(Mini_Dog_Data.Mode == MINI_DOG_STABLE)
	{
		float set_val = 0;
		pid->error = set_val - mpu_val;
		pid->diff = pid->error - pid->last_error;
		pid->integral += pid->error;
		pid->output = (pid->kp * pid->error) + (pid->kd * pid->diff) + (pid->ki * pid->integral);
		if(pid->integral>100)
		{
			pid->integral = 100;
		}
		if(pid->integral<-100)
		{
			pid->integral = -100;
		}
		target_val = pid->output;
		pid->last_error = pid->error;
		
		return target_val;
	}else{
		return target_val;
	}
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Pid_Calculation
	* @ 功能说明： 机器狗PID计算
  * @ 参数    ： float target_val 目标值
  * @ 参数    ： float current_val 当前值
  * @ 返回值  ： NULL
  *****************************************************************/
float Mini_Dog_Pid_Calculation(float target_val, float current_val)
{
	Mini_Dog_Pid_Struct p = {0};
	p.kp = 0.8;
	p.ki = 0;
	p.kd = 0;
	p.error = target_val- current_val;
	p.output = p.kp * p.error;
	current_val = current_val + p.output;
	
	return current_val;
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Attitude_To_Coordinate
  * @ 功能说明： 机器狗姿态解算为实际坐标系
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Attitude_To_Coordinate(const Mini_Dog_Attitude_Struct *attitude, float x[], float z[])
{
	// 机械结构数据
	float length = Mini_Dog_Data.Body_Length;
	float width = Mini_Dog_Data.Body_Width;
	
	// 姿态角
	float pitch = attitude->pitch * MINI_DOG_PI / 180.f; // 俯仰角
	float roll = attitude->roll * MINI_DOG_PI / 180.f; // 滚转角
	float yaw;
	
	// 计算1、2、3、4腿坐标
	x[0] = length/2 - attitude->x - (length*cos(pitch)*cos(yaw))/2 - (width*cos(pitch)*sin(yaw))/2;
	z[0] = (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 - attitude->z - (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;
	
	x[1] = (length*cos(pitch)*cos(yaw))/2 - attitude->x - length/2 - (width*cos(pitch)*sin(yaw))/2;
	z[1] = (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 - attitude->z + (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;
	
	x[2] = length/2 - attitude->x - (length*cos(pitch)*cos(yaw))/2 + (width*cos(pitch)*sin(yaw))/2;
	z[2] = -attitude->z - (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 - (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;
	
	x[3] = (length*cos(pitch)*cos(yaw))/2 - attitude->x - length/2 + (width*cos(pitch)*sin(yaw))/2;
	z[3] = -attitude->z - (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 + (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Coordinate_To_Angle
  * @ 功能说明： 机器狗坐标解算为实际舵机角度
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Coordinate_To_Angle(const float x[], const float z[], float thigh[], float calf[])
{
	// 机械结构数据
	float thigh_length = Mini_Dog_Data.Thigh_Length;
	float calf_length = Mini_Dog_Data.Calf_Length;
	
	// 角度数据
	float angle;
	
	//计算1、2、3、4大小腿角度
	angle = sqrt(x[0] * x[0] + z[0] * z[0]);
	calf[0] = acos((angle*angle - thigh_length * thigh_length - calf_length * calf_length) / ( 2 * thigh_length * calf_length));
	thigh[0] = MINI_DOG_PI - acos(x[0] / angle) - acos((thigh_length * thigh_length + angle * angle - calf_length * calf_length) / (2 * thigh_length * angle));
	
	angle = sqrt(x[1] * x[1] + z[1] * z[1]);
	calf[1] = acos((angle * angle - thigh_length * thigh_length - calf_length * calf_length)/(2 * thigh_length * calf_length));
	thigh[1] = MINI_DOG_PI - acos(x[1] / angle) - acos((thigh_length * thigh_length + angle * angle - calf_length * calf_length)/(2 * thigh_length * angle));
	
	angle = sqrt(x[2] * x[2] + z[2] * z[2]);
	calf[2] = acos((angle * angle - thigh_length * thigh_length - calf_length * calf_length) / (2 * thigh_length * calf_length));
	thigh[2] = MINI_DOG_PI - acos(x[2] / angle) - acos((thigh_length * thigh_length + angle * angle - calf_length * calf_length)/(2 * thigh_length * angle));
	
	angle = sqrt(x[3] * x[3] + z[3] * z[3]);
	calf[3] = acos((angle * angle - thigh_length * thigh_length - thigh_length * thigh_length)/(2 * thigh_length * calf_length));
	thigh[3] = MINI_DOG_PI - acos(x[3] / angle) - acos((thigh_length * thigh_length + angle * angle - calf_length * calf_length)/(2 * thigh_length * angle));
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Data_To_Servo
	* @ 功能说明： 机器狗角度数据发送至舵机
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Data_To_Servo(float thigh[], float calf[], uint16_t zero_position[], uint16_t speed)
{
	uint8_t  servo_id[MINI_DOG_LEN];
	uint16_t servo_position[MINI_DOG_LEN];
	uint16_t servo_speed[MINI_DOG_LEN];
	
	int servo_num = sizeof(servo_id)/sizeof(servo_id[0]);
	
	// 角度值转换->弧度->舵机数字信号
	for(int i=0;i<servo_num/2;i++)
	{
		thigh[i] = thigh[i] * 180.0f / MINI_DOG_PI * MINI_DOG_ANGLE_TO_DIGITAL;
		calf[i] = calf[i] * 180.0f / MINI_DOG_PI * MINI_DOG_ANGLE_TO_DIGITAL;
	}
	
	//设置舵机ID和速度值
	for(int i=0;i<servo_num;i++)
	{
		servo_id[i] = i + 1;
		servo_speed[i] = speed;
	}
	
	// 通过校准后的零位值换算小腿的位置数据
	servo_position[0] = zero_position[0] + (int32_t)calf[0];
	servo_position[1] = zero_position[1] + (int32_t)calf[1];
	servo_position[2] = zero_position[2] - (int32_t)calf[2];
	servo_position[3] = zero_position[3] - (int32_t)calf[3];
	
	// 通过校准后的零位值换算大腿的位置数据
	servo_position[4] = zero_position[4] - (int32_t)thigh[0];
	servo_position[5] = zero_position[5] - (int32_t)thigh[1];
	servo_position[6] = zero_position[6] + (int32_t)thigh[2];
	servo_position[7] = zero_position[7] + (int32_t)thigh[3];
	
	// 发送舵机位置控制数据
	Servo_Write_Position(servo_id, servo_num, servo_position, 0, servo_speed);
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Gait_Generate_Time
  * @ 功能说明： 步态时序
  * @ 参数    ： float *gait_time 步态时序
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Gait_Generate_Time(float *gait_time, float it, float n)
{
	// 消除了1、4腿的抖动问题
	*gait_time = *gait_time + it;
	if(*gait_time > n * MINI_DOG_TS) {
		*gait_time = it;
	}
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Trot_Gait_Generate
  * @ 功能说明： 生成Trot步态
  * @ 参数    ： float gait_time 步态时序
  * @ 参数    ： float step_width 步宽
  * @ 参数    ： float lift_height 足端抬起高度
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Trot_Gait_Generate(float gait_time, float step_width, float lift_height,  float x[], float z[])
{
	float sigma;
	float zep_b, xep_b; 
	float xep_z; 		
	float xs = -step_width * 0.8f;
	float xf = step_width * 0.2f;
	int8_t right, left = 0;
	
	// 校验控制模式
	switch(Mini_Dog_Data.Motion_Mode)
	{
		case MINI_DOG_FORWARD:
			right = 1; left = 1;
			break;
		case MINI_DOG_BACK:
			right = -1; left = -1; 
			break;
		case MINI_DOG_STEP:
			right = 0; left = 0; 
			break;
		case MINI_DOG_RIGHT:
			right = -1; left = 1; 
			break;
		case MINI_DOG_LEFT:
			right = 1; left = -1; 
			break;
		case MINI_DOG_STOP:
			right = 0; left = 0; 
		  lift_height = 0;
			break;
		default:
			right = 0; left = 0;
			lift_height = 0;
			break;
	}
	
	// 生成步态
	if (gait_time <= MINI_DOG_TS * MINI_DOG_FAI)
	{
		sigma = 2 * MINI_DOG_PI * gait_time / (MINI_DOG_FAI * MINI_DOG_TS);
		zep_b = lift_height * (1 - cos(sigma)) / 2;
		xep_b = (xf - xs) * ((sigma - sin(sigma))/(2 * MINI_DOG_PI)) + xs;
		xep_z = (xs - xf) * ((sigma - sin(sigma))/(2 * MINI_DOG_PI)) + xf;
		
		z[0] = z[0] + zep_b;
		z[1] = z[1];
		z[2] = z[2];
    z[3] = z[3] + zep_b;
		
		x[0] = x[0] + xep_b * right;
		x[1] = x[1] + xep_z * right;
		x[2] = x[2] + xep_z * left;
		x[3] = x[3] + xep_b * left;
	}
	
	if (gait_time > MINI_DOG_TS * MINI_DOG_FAI && gait_time < MINI_DOG_TS)
	{
		sigma = 2 * MINI_DOG_PI * (gait_time - MINI_DOG_TS * MINI_DOG_FAI) / (MINI_DOG_FAI * MINI_DOG_TS);
		
		zep_b = lift_height * (1 - cos(sigma)) / 2;
		xep_b = (xf - xs)*((sigma - sin(sigma))/(2 * MINI_DOG_PI)) + xs;
		
		xep_z = (xs - xf)*((sigma - sin(sigma))/(2 * MINI_DOG_PI)) + xf;
		
		z[0] = z[0];
		z[1] = z[1] + zep_b;
		z[2] = z[2] + zep_b;
    z[3] = z[3];
		
		x[0] = x[0] + xep_z * right;
		x[1] = x[1] + xep_b * right;
		x[2] = x[2] + xep_b * left;
		x[3] = x[3] + xep_z * left;
	}
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Trot_Walk_Generate
  * @ 功能说明： 生成Walk步态
  * @ 参数    ： float gait_time 步态时序
  * @ 参数    ： float step_width 步宽
  * @ 参数    ： float lift_height 足端抬起高度
  * @ 返回值  ： NULL
  *****************************************************************/
int32_t Mini_Dog_Walk_Gait_Generate_Plan(int32_t xs, int32_t xf, float t)
{
	int32_t ret;
	ret = xs + (xf-xs) * 2 * t;
	return ret;
}

void Mini_Dog_Walk_Gait_Generate(float gait_time, float step_width, float lift_height,  float x[], float z[])
{
	float sigma;
	float zep_b, xep_b;
	float xs = 0;
	float xf = step_width;
	
	// 校验控制模式
	switch(Mini_Dog_Data.Motion_Mode)
	{
		case MINI_DOG_FORWARD:
			break;
		case MINI_DOG_STOP:
		  lift_height = 0;
			break;
		default:
			lift_height = 0;
			break;
	}
	
	// 生成步态
	if(lift_height != 0){
		// 规划重心
		if(gait_time <= MINI_DOG_TS * MINI_DOG_FAI) {
			Mini_Dog_Data.Target.x = Mini_Dog_Walk_Gait_Generate_Plan(0,xf/2,gait_time);
		}
		if(gait_time>MINI_DOG_TS*MINI_DOG_FAI && gait_time<=2*MINI_DOG_TS*MINI_DOG_FAI) 
		{
			// 迈腿4
			gait_time = gait_time - MINI_DOG_TS*MINI_DOG_FAI;
			sigma = 2*MINI_DOG_PI*gait_time/(MINI_DOG_FAI*MINI_DOG_TS);
			zep_b = lift_height*(1-cos(sigma))/2;
			xep_b = (xf-xs)*((sigma-sin(sigma))/(2*MINI_DOG_PI))+xs;
			z[3] = z[3]+zep_b;
			x[3] = x[3]+xep_b;
		}
		
		// 规划重心
		if(gait_time>2*MINI_DOG_TS*MINI_DOG_FAI && gait_time<=3*MINI_DOG_TS*MINI_DOG_FAI) {
			gait_time = gait_time - 2*MINI_DOG_TS*MINI_DOG_FAI;		
			Mini_Dog_Data.Target.x = Mini_Dog_Walk_Gait_Generate_Plan(xf/2,0,gait_time);
			x[3] = x[3]+xf; 
		}
		if(gait_time>3*MINI_DOG_TS*MINI_DOG_FAI && gait_time<=4*MINI_DOG_TS*MINI_DOG_FAI)
		{		
			// 迈腿3
			gait_time = gait_time - 3*MINI_DOG_TS*MINI_DOG_FAI;
			sigma = 2*MINI_DOG_PI*gait_time/(MINI_DOG_FAI*MINI_DOG_TS);
			zep_b = lift_height*(1-cos(sigma))/2;
			xep_b = (xf-xs)*((sigma-sin(sigma))/(2*MINI_DOG_PI))+xs;
			z[2] = z[2]+zep_b;
			x[2] = x[2]+xep_b;
			x[3] = x[3]+xf;
		}
		
		// 规划重心
		if(gait_time>4*MINI_DOG_TS*MINI_DOG_FAI && gait_time<=5*MINI_DOG_TS*MINI_DOG_FAI) {
			gait_time = gait_time - 4*MINI_DOG_TS*MINI_DOG_FAI;
			Mini_Dog_Data.Target.x = Mini_Dog_Walk_Gait_Generate_Plan(0,xf,gait_time);
			x[2] = x[2]+xf;
			x[3] = x[3]+xf;
		}
		if(gait_time>5*MINI_DOG_TS*MINI_DOG_FAI && gait_time<=6*MINI_DOG_TS*MINI_DOG_FAI)
		{
			// 迈腿2
			gait_time = gait_time - 5*MINI_DOG_TS*MINI_DOG_FAI;
			sigma = 2*MINI_DOG_PI*gait_time/(MINI_DOG_FAI*MINI_DOG_TS);
			zep_b = lift_height*(1-cos(sigma))/2;
			xep_b = (xf-xs)*((sigma-sin(sigma))/(2*MINI_DOG_PI))+xs;
			z[1] = z[1]+zep_b;
			x[1] = x[1]+xep_b;
			x[2] = x[2]+xf;
			x[3] = x[3]+xf;
		}
		
		// 规划重心		
		if(gait_time>6*MINI_DOG_TS*MINI_DOG_FAI && gait_time<=7*MINI_DOG_TS*MINI_DOG_FAI) {
			gait_time = gait_time - 6*MINI_DOG_TS*MINI_DOG_FAI;	
			Mini_Dog_Data.Target.x = Mini_Dog_Walk_Gait_Generate_Plan(xf,xf/2,gait_time);
			x[1] = x[1]+xf;
			x[2] = x[2]+xf;
			x[3] = x[3]+xf;	
		}

		if(gait_time>7*MINI_DOG_TS*MINI_DOG_FAI && gait_time<=8*MINI_DOG_TS*MINI_DOG_FAI)
		{
			//迈腿1
			gait_time = gait_time - 7*MINI_DOG_TS*MINI_DOG_FAI;	
			sigma = 2*MINI_DOG_PI*gait_time/(MINI_DOG_FAI*MINI_DOG_TS);
			zep_b = lift_height*(1-cos(sigma))/2;
			xep_b = (xf-xs)*((sigma-sin(sigma))/(2*MINI_DOG_PI))+xs;
			z[0] = z[0]+zep_b;
			x[0] = x[0]+xep_b;
			x[1] = x[1]+xf;
			x[2] = x[2]+xf;
			x[3] = x[3]+xf;
		}
		
		// 规划重心	
		if(gait_time>8*MINI_DOG_TS*MINI_DOG_FAI && gait_time<=9*MINI_DOG_TS*MINI_DOG_FAI) {
			gait_time = gait_time - 8*MINI_DOG_TS*MINI_DOG_FAI;	
			Mini_Dog_Data.Target.x = Mini_Dog_Walk_Gait_Generate_Plan(xf/2,xf,gait_time);
			x[0] = x[0]+xf;
			x[1] = x[1]+xf;
			x[2] = x[2]+xf;
			x[3] = x[3]+xf;			
		}
		
		if(gait_time>9*MINI_DOG_TS*MINI_DOG_FAI)
		{
			x[0] = x[0]+xf;
			x[1] = x[1]+xf;
			x[2] = x[2]+xf;
			x[3] = x[3]+xf;	
			Mini_Dog_Data.Target.x = 0;
			Mini_Dog_Data.Current.x = 0;
		}
	}
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Trot_Pronk_Generate
  * @ 功能说明： 生成Pronk步态
  * @ 参数    ： float gait_time 步态时序
  * @ 参数    ： Mini_Dog_Motion_Enum motion 运动模式
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Trot_Pronk_Generate(float gait_time, Mini_Dog_Motion_Enum motion, float x[], float z[])
{
	float prepare_time = 0.7; // 收腿蓄力时间
	float jump_time = 0.3; // 伸腿跳跃时间
	float tuck_length = 50; // 收腿长度
	float stretch_length = 100; // 伸腿长度
	
	if(motion == MINI_DOG_FORWARD)
	{
		// 向前跳
		if(gait_time <= prepare_time){
			for(int i=0;i<4;i++) { 
				x[i] = -10;
				z[i] = -tuck_length;
			}
		}else{
			if((gait_time > prepare_time) && gait_time <= (prepare_time + jump_time))
			{
				for(int i=0;i<4;i++) {
					x[i] = -40;
					z[i] = -stretch_length;
				}
			}
		}
	}
	if(motion == MINI_DOG_BACK)
	{
		// 向后跳
		if(gait_time <= prepare_time)
		{
			for(int i=0;i<4;i++) {
				x[i] = -30;
				z[i] = -tuck_length;
			}
		}else{
			if((gait_time > prepare_time) && gait_time<=(prepare_time + jump_time)){
				for(int i=0;i<4;i++) {
					x[i] = 10;
					z[i] = -stretch_length;
				}
			}
		}
	}
	if(motion == MINI_DOG_STEP)
	{
		// 原地跳
		if(gait_time <= prepare_time)
		{
			for(int i=0;i<4;i++) { 
				x[i] = -30;
				z[i] = -tuck_length;
			}
		}else{
			if((gait_time > prepare_time) && gait_time <= (prepare_time + jump_time))
			{
				for(int i=0;i<4;i++) {
					x[i] = -30;
					z[i] = -stretch_length;
				}
			}
		}
	}
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Remote_Control
  * @ 功能说明： 机器狗遥控模式
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Remote_Control(void)
{
	// 俯仰角自稳
	Mini_Dog_Data.Target.pitch = Mini_Dog_Stable_Calculation(Mpu_Read_Data.pit, Mini_Dog_Data.Target.pitch, &Mini_Dog_Data.Pitch_Pid);
	// 滚转角自稳
	Mini_Dog_Data.Target.roll = Mini_Dog_Stable_Calculation(Mpu_Read_Data.rol, Mini_Dog_Data.Target.roll, &Mini_Dog_Data.Roll_Pid);
	// 高度PID计算
	Mini_Dog_Data.Current.z = Mini_Dog_Pid_Calculation(Mini_Dog_Data.Target.z, Mini_Dog_Data.Current.z);
	// 重心PID计算
	Mini_Dog_Data.Current.x = Mini_Dog_Pid_Calculation(Mini_Dog_Data.Target.x, Mini_Dog_Data.Current.x);
	// 俯仰角PID计算
	Mini_Dog_Data.Current.pitch = Mini_Dog_Pid_Calculation(Mini_Dog_Data.Target.pitch, Mini_Dog_Data.Current.pitch);
	// 滚转角PID计算
	Mini_Dog_Data.Current.roll = Mini_Dog_Pid_Calculation(Mini_Dog_Data.Target.roll, Mini_Dog_Data.Current.roll);
	
	// 姿态转坐标系
	Mini_Dog_Attitude_To_Coordinate(&Mini_Dog_Data.Current, Mini_Dog_Data.Foot_Coordinate.x, Mini_Dog_Data.Foot_Coordinate.z);
	
	// 步态控制
	switch(Mini_Dog_Data.Gait_Mode)
	{
		case MINI_DOG_TROT:
			// Trot步态
			Mini_Dog_Gait_Generate_Time(&Mini_Dog_Data.Gait_Time, 0.05, 1);
			Mini_Dog_Trot_Gait_Generate(Mini_Dog_Data.Gait_Time, 35, 50, Mini_Dog_Data.Foot_Coordinate.x, Mini_Dog_Data.Foot_Coordinate.z);
			break;
		case MINI_DOG_WALK:
			// Walk步态
			Mini_Dog_Gait_Generate_Time(&Mini_Dog_Data.Gait_Time, 0.025, 4.5);
			Mini_Dog_Walk_Gait_Generate(Mini_Dog_Data.Gait_Time, 35, 45, Mini_Dog_Data.Foot_Coordinate.x, Mini_Dog_Data.Foot_Coordinate.z);
			break;
		case MINI_DOG_PRONK:
			// PRONK步态
			Mini_Dog_Gait_Generate_Time(&Mini_Dog_Data.Gait_Time, 0.035, 1);
			Mini_Dog_Trot_Pronk_Generate(Mini_Dog_Data.Gait_Time, Mini_Dog_Data.Motion_Mode, Mini_Dog_Data.Foot_Coordinate.x, Mini_Dog_Data.Foot_Coordinate.z);
			break;
		case MINI_DOG_BOUND:
			break;
	}
	
	// 坐标系转舵机角度
	Mini_Dog_Coordinate_To_Angle(Mini_Dog_Data.Foot_Coordinate.x, Mini_Dog_Data.Foot_Coordinate.z, Mini_Dog_Data.Thigh, Mini_Dog_Data.Calf);
	
	// 角度发送到舵机信号
	Mini_Dog_Data_To_Servo(Mini_Dog_Data.Thigh, Mini_Dog_Data.Calf, Mini_Dog_Data.Zero_Position, 1500);
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Calibration
  * @ 功能说明： 机器狗标定模式
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Calibration(uint32_t id, uint32_t position)
{
	Servo_Id_Write_Position(Mini_Dog_Data.Coordinate_Position.servo_id, Mini_Dog_Data.Coordinate_Position.servo_position, 0, 500);
	if(id==1 || id==2 || id==7 || id==8) {
		Mini_Dog_Data.Zero_Position[id-1] = position - MINI_DOG_OFFSET;
	}
	if(id==3 || id==4 || id==5 || id==6) {
		Mini_Dog_Data.Zero_Position[id-1] = position + MINI_DOG_OFFSET;
	}
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Action
  * @ 功能说明： 机器狗动作模式
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Action(void)
{
	
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Attitude_Interface
  * @ 功能说明： 机器狗姿态控制接口
  * @ 参数    ： char *value 姿态控制值
  * @ 参数    ： float current_value 当前值
  * @ 参数    ： float arget_value 目标值
  * @ 参数    ： uint16_t time 周期时间
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Attitude_Interface(char *value, float current_value, float target_value, uint16_t time)
{
	// 周期时间限幅
	if(time > 1000)
	{
		time = 1000;
	}
	if(time < 100){
		time = 100;
	}
	
	uint16_t step_count = time / 10;
	float step = (target_value - current_value) / step_count;
	float *p = NULL;
	
	// 姿态控制
	if(strcmp(value, "x") == 0) {
		p = &Mini_Dog_Data.Current.x;
	}
	if(strcmp(value, "z") == 0) {
		p = &Mini_Dog_Data.Current.z;
	}
	if(strcmp(value, "roll") == 0) {
		p = &Mini_Dog_Data.Current.roll;
	}
	if(strcmp(value, "pitch") == 0) {
		p = &Mini_Dog_Data.Current.pitch;
	}
	
	// 分步执行
	for(int i=0;i<step_count;i++)
	{
		*p += step;
		// 姿态转坐标系
		Mini_Dog_Attitude_To_Coordinate(&Mini_Dog_Data.Current, Mini_Dog_Data.Foot_Coordinate.x, Mini_Dog_Data.Foot_Coordinate.z);
		// 坐标系转舵机角度
		Mini_Dog_Coordinate_To_Angle(Mini_Dog_Data.Foot_Coordinate.x, Mini_Dog_Data.Foot_Coordinate.z, Mini_Dog_Data.Thigh, Mini_Dog_Data.Calf);
		// 角度发送到舵机信号
		Mini_Dog_Data_To_Servo(Mini_Dog_Data.Thigh, Mini_Dog_Data.Calf, Mini_Dog_Data.Zero_Position, 1500);
		delay_ms(10);
	}
	Mini_Dog_Data.Status = MINI_DOG_INIT_TRUE;
}

/******************************************************************
  * @ 函数名  ： Mini_Dog_Task
  * @ 功能说明： 机器狗任务
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Mini_Dog_Task(void)
{
	// 完成初始化后执行任务
	if(Mini_Dog_Data.Status == MINI_DOG_INIT_TRUE)
	{
		// 根据控制模式执行具体业务
		switch(Mini_Dog_Data.Mode)
		{
			case MINI_DOG_REMOTE_CONTROL:
				// 遥控模式
				Mini_Dog_Remote_Control();
				break;
			case MINI_DOG_CALIBRATION:
				// 标定模式
				Mini_Dog_Calibration(Mini_Dog_Data.Coordinate_Position.servo_id, Mini_Dog_Data.Coordinate_Position.servo_position);
				break;
			case MINI_DOG_ACTION:
				// 动作模式
				Mini_Dog_Action();
				break;
			case MINI_DOG_STABLE:
				Mini_Dog_Remote_Control();
				break;
			default:
				Mini_Dog_Remote_Control();
		}
	}
	delay_ms(Mini_Dog_Data.Gait_Cycle);
}

/*******************************************************************************
 * @funtion      : Mini_Dog_Usb_Callback
 * @description  : 串口任务回调函数
 * @param         {char *type} 通讯协议类型
 * @param         {int mode} 运动模式
 * @return        {*}
 *******************************************************************************/
void Mini_Dog_Usb_Callback(char *type, int mode, int motion, int cycle, int id, int position)
{
   if (memcmp(type, "mini-dog-mode", 13) == 0)
	 {
		 Mini_Dog_Data.Mode = (Mini_Dog_Mode_Enum)mode;
	 }
	 if (memcmp(type, "mini-dog-motion", 15) == 0)
	 {
		 Mini_Dog_Data.Motion_Mode = (Mini_Dog_Motion_Enum)motion;
	 }
	 if (memcmp(type, "mini-dog-cycle", 14) == 0)
	 {
		 Mini_Dog_Data.Gait_Cycle = (uint32_t)cycle;
	 }
	 if (memcmp(type, "mini-dog-init", 13) == 0)
	 {
		 // 机器狗进入停止运动模式
		 Mini_Dog_Data.Mode = MINI_DOG_CALIBRATION;
		 Mini_Dog_Data.Motion_Mode = MINI_DOG_STOP;
	 }
	 if (memcmp(type, "mini-dog-id", 11) == 0)
	 {
		 Mini_Dog_Data.Coordinate_Position.servo_id = id;
		 Mini_Dog_Data.Coordinate_Position.servo_position = position;
	 }
	 if (memcmp(type, "mini-dog-finish", 15) == 0)
	 {
        if(Mini_Dog_Data.Mode == MINI_DOG_CALIBRATION){
            if(Flash_Write(8, Mini_Dog_Data.Zero_Position) != FLASH_SUCCESS)
            {
                Usb_Write_Data("{\"type\":\"mini-dog-finish-error\"}\r\n");
            }else{
                Usb_Write_Data("{\"type\":\"mini-dog-finish-success\"}\r\n");
            }
        }
	 }
}
